#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
系统信息与监控节点

该节点是一个非核心的工具性节点，主要提供两个功能：
1.  **启动信息展示**: 在节点启动时，向控制台打印一份详细的、对用户友好的
    系统概览。内容包括系统架构、关键ROS话题、修复日志以及常用的监控命令，
    帮助用户快速了解系统状态和进行调试。
2.  **状态监控**: 作为一个后台守护进程，它会订阅关键节点（ASR, TTS, 导航）
    的状态话题，并周期性地检查系统健康状况，如关键话题是否缺失，并
    定时汇报各子系统的状态。
"""

import rospy
import time
from std_msgs.msg import String

class SystemInfo:
    """
    封装了系统信息显示和后台监控逻辑的类。
    """
    def __init__(self):
        """
        初始化SystemInfo节点。
        """
        rospy.init_node('system_info', anonymous=True)
        
        # --- 内部状态变量 ---
        # 用于存储从其他节点接收到的最新状态
        self.asr_status = "未知"
        self.tts_status = "未知"
        self.nav_status = "未知"
        
        # --- ROS Subscribers ---
        # 订阅关键节点的状态话题
        rospy.Subscriber('asr_status', String, self.asr_status_callback)
        rospy.Subscriber('tts_status', String, self.tts_status_callback)
        rospy.Subscriber('navigation_status', String, self.nav_status_callback)
        
        # 节点启动后，立即显示一次系统信息，并开始后台监控
        self.show_system_info()
        self.monitor_system()
    
    def asr_status_callback(self, msg):
        """更新ASR状态的回调函数。"""
        self.asr_status = msg.data
    
    def tts_status_callback(self, msg):
        """更新TTS状态的回调函数。"""
        self.tts_status = msg.data
    
    def nav_status_callback(self, msg):
        """更新导航状态的回调函数。"""
        self.nav_status = msg.data
    
    def show_system_info(self):
        """
        在ROS日志中打印一次详细的系统信息，作为用户启动向导。
        """
        rospy.loginfo("=" * 60)
        rospy.loginfo("=============== 盲人引导机器人系统已启动 ===============")
        rospy.loginfo("=" * 60)
        rospy.loginfo("系统架构：")
        rospy.loginfo("  > 核心节点: center_node (负责NLU和任务分发)")
        rospy.loginfo("  > 语音服务: asr.py (语音识别) + tts.py (语音合成)")
        rospy.loginfo("  > 功能模块: nav_controller.py, robot_control.py, arm_control.py, visual_recognition.py")
        rospy.loginfo("=" * 60)
        rospy.loginfo("关键话题 (Topics)：")
        rospy.loginfo("  - /raw_text          - ASR识别出的原始文本")
        rospy.loginfo("  - /tts_text          - 需要TTS合成的文本")
        rospy.loginfo("  - /tts_status        - TTS播报状态 (playing/finished)")
        rospy.loginfo("  - /navigation_command - 发往导航系统的目标指令")
        rospy.loginfo("  - /arm_command       - 发往机械臂的控制指令")
        rospy.loginfo("  - /visual_command    - 发往视觉识别系统的指令")
        rospy.loginfo("  - /robot_command     - 发往底盘的手动控制指令")
        rospy.loginfo("=" * 60)
        rospy.loginfo("系统特性：")
        rospy.loginfo("  [✓] ASR由中央节点服务化控制，按需启停。")
        rospy.loginfo("  [✓] TTS与ASR严格互斥，防止机器人自我识别。")
        rospy.loginfo("  [✓] 导航与ASR互斥，防止行进中误识别。")
        rospy.loginfo("  [✓] 统一的JSON指令格式，由LLM生成。")
        rospy.loginfo("=" * 60)
        rospy.loginfo("常用监控命令：")
        rospy.loginfo("  $ rostopic list                 - 查看所有话题")
        rospy.loginfo("  $ rostopic echo /raw_text       - 监控ASR识别结果")
        rospy.loginfo("  $ rostopic echo /tts_status     - 监控TTS状态")
        rospy.loginfo("  $ rostopic echo /robot_command  - 监控运动指令")
        rospy.loginfo("  $ rosnode list                  - 查看所有节点")
        rospy.loginfo("=" * 60)
        rospy.loginfo(">>>>>>>>>>>>>> 系统已就绪，可以开始使用语音指令！ <<<<<<<<<<<<<<")
        rospy.loginfo("=" * 60)
    
    def monitor_system(self):
        """
        以低频率运行一个监控循环，检查系统健康状况并汇报。
        """
        # 设置一个较低的更新频率，例如每10秒检查一次
        rate = rospy.Rate(0.1)
        
        while not rospy.is_shutdown():
            try:
                # 检查关键话题是否都处于活动状态
                published_topics = rospy.get_published_topics()
                active_topic_names = [topic[0] for topic in published_topics]
                
                key_topics = [
                    '/raw_text', '/tts_text', '/tts_status', 
                    '/navigation_command', '/arm_command', '/visual_command'
                ]
                
                missing_topics = [topic for topic in key_topics if topic not in active_topic_names]
                
                if missing_topics:
                    rospy.logwarn(f"系统健康检查：检测到以下关键话题缺失: {missing_topics}")
                
                # 每分钟在日志中打印一次聚合的状态信息
                if int(time.time()) % 60 == 0:
                    status_report = f"系统状态 | ASR: {self.asr_status}, TTS: {self.tts_status}, 导航: {self.nav_status}"
                    rospy.loginfo(status_report)
                
            except Exception as e:
                rospy.logwarn(f"系统监控线程发生错误: {e}")
            
            rate.sleep()

if __name__ == '__main__':
    try:
        system_info_node = SystemInfo()
    except rospy.ROSInterruptException:
        rospy.loginfo("系统信息节点关闭。") 